from machine import UART
import sensor, image, time

# 初始化激光灯控制引脚，并置为高电平
#laser_light=Pin("C9", Pin.OUT)
#laser_light.value(1)
uart = UART(1, baudrate=115200)     # 初始化串口 波特率设置为115200 TX是B12 RX是B13
# 初始化TFT180屏幕
#lcd = seekfree.LCD180(3)


class target_check(object):
    para1 = 0   #uin8
    para2 = 0   #uin8
    para3 = 0   #uin8
    para4 = 0   #uin16
    para5 = 0   #uin16
    para6 = 0   #uin16
    para7 = 0   #in32
    para8 = 0   #in32
    para9 = 0   #in32

pack=target_check()

class uart_buf_prase(object):
    uart_buf = []
    _data_len = 0
    _data_cnt = 0
    state = 0

HEADER=[0x11,0x13]
MODE=[0x01,0x02,0x03]

def package_blobs_data():
    #数据打包封装
    data=bytearray([HEADER[0],HEADER[1],MODE[0],0x00,
                   pack.para1,
                   pack.para2,
                   pack.para3,
                   pack.para4>>8,pack.para4&0xff,
                   pack.para5>>8,pack.para5&0xff,
                   pack.para6>>8,pack.para6&0xff,
                   pack.para7>>24&0xff,pack.para7>>16&0xff,
                   pack.para7>>8&0xff,pack.para7&0xff,
                   pack.para8>>24&0xff,pack.para8>>16&0xff,
                   pack.para8>>8&0xff,pack.para8&0xff,
                   pack.para9>>24&0xff,pack.para9>>16&0xff,
                   pack.para9>>8&0xff,pack.para9&0xff,
                   0x00])
    #数据包的长度
    data_len=len(data)
    data[3]=data_len-5#有效数据的长度
    #和校验
    sum=0
    for i in range(0,data_len-1):
        sum=sum+data[i]
    data[data_len-1]=sum%256
    #返回打包好的数据
    return data


# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565) # 设置图像色彩格式为RGB565格式
sensor.set_framesize(sensor.QVGA)  # 设置图像大小为160*120
sensor.set_auto_whitebal(False)      # 设置自动白平衡
sensor.set_brightness(400)         # 设置亮度为3000  150
sensor.skip_frames(time = 20)       # 跳过帧


clock = time.clock()

red_x = 0
red_y = 0
green_x = 0
green_y = 0
err_x = 0
err_y = 0


while(True):
    clock.tick()
    img = sensor.snapshot()#.lens_corr(strength = 1.5, zoom = 1.0)


    # 打印帧率
# -----跟踪激光部分-----
    # 设置激光颜色阈值
    red_td = [(0, 127, 26, 127, -128, 127)]
    # 根据阈值找到色块
    for b in img.find_blobs(red_td,pixels_threshold=1, area_threshold=1, merge=False,invert = 0,roi = (68,24,191,181)):
        # 在屏幕上画出色块
        img.draw_rectangle(b.rect(), color = (0, 255, 0), scale = 2, thickness = 1)
        red_x = b.x()+int(b.w()/2)
        red_y = b.y()+int(b.h()/2)

    # 设置激光颜色阈值
    green_td = [(0, 100, -13, -65, -18, 66)]
    # 根据阈值找到色块
    for b in img.find_blobs(green_td,pixels_threshold=1, area_threshold=1, merge=False,invert = 0,roi = (97,69,104,106)):
        # 在屏幕上画出色块
        #img.draw_rectangle(b.rect(), color = (0, 0, 255), scale = 6, thickness = 1)

        green_x = b.x()+int(b.w()/2)
        green_y = b.y()+int(b.h()/2)
        img.draw_circle(green_x, green_y, 5, color = (0, 0, 255), thickness = 2, fill = False)
        break

    err_x = red_x - green_x
    err_y = red_y - green_y
    pack.para8 = err_x
    pack.para9 = err_y
    print(err_x,err_y)
    uart.write(package_blobs_data())
